# !/usr/bin/env python3
# -*- coding: utf-8 -*-
# @Time    : 2025/07/22 14:00
# @Author  : Yida Hao
# @File    : set_joint_cmd.py

from rclpy.node import Node
from interfaces.srv import SetJointCommand
from sensor_msgs.msg import JointState

topic_name = 'joint_commands'

SRV_NAME = 'set_joint_command_service'

"""
Service to set joint commands for the robot.
"""

class SetJointCommandService(Node):
    def __init__(self):
        super().__init__(f'{SRV_NAME}_srv')
        self.init_joint_command_publisher()
        self.srv = self.create_service(SetJointCommand, SRV_NAME, self.srv_callback)
        self.get_logger().info("service ready")

    def init_joint_command_publisher(self):
        self.pub = self.create_publisher(JointState, topic_name, 10)

    def srv_callback(self, request, response):
        self.pub.publish(request.joint_command)
        self.get_logger().info(f"service called, publish position: {request.joint_command.position}")
    
        return response

class SetJointCommandClient(Node):
    def __init__(self):
        super().__init__(f'{SRV_NAME}_cli')
        self.cli = self.create_client(SetJointCommand, SRV_NAME)

        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting ...')

        self.req = SetJointCommand.Request()

    def send_request(self, command):
        if command is not None:
            self.req.joint_command = command
        future = self.cli.call_async(self.req)
        return future